
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_yaw_controller.h
  * @author     baiyang
  * @date       2023-6-30
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mp_autotune.h"

#include <pid/pid.h>
#include <pid/pid_info.h>

#include <vehicle/mb_fixed_wing.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct mp_yaw_controller* mp_yaw_controller_t;

struct mp_yaw_controller {
    mb_fixed_wing_const_t aparm;

    Param_float _K_A;
    Param_float _K_I;
    Param_float _K_D;
    Param_float _K_FF;
    Param_int16 _imax;
    Param_int8  _rate_enable;
    PID_ctrl rate_pid;

    uint32_t _last_t;
    float _last_out;
    float _last_rate_hp_out;
    float _last_rate_hp_in;
    float _K_D_last;

    float _integrator;
    float _dt;

    struct ATGains gains;
    mp_autotune_t autotune;
    bool failed_autotune_alloc;

    struct pid_info _pid_info;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void yctrl_ctor(mp_yaw_controller_t yaw_ctrl, mb_fixed_wing_const_t parms, float dt);

// get actuator output for sideslip and yaw damping control
int32_t yctrl_get_servo_out(mp_yaw_controller_t yaw_ctrl, float scaler, bool disable_integrator);

// get actuator output for direct rate control
// desired_rate is in deg/sec. scaler is the surface speed scaler
float yctrl_get_rate_out(mp_yaw_controller_t yaw_ctrl, float desired_rate, float scaler, bool disable_integrator);

void yctrl_reset_i(mp_yaw_controller_t yaw_ctrl);
void yctrl_reset_rate_pid(mp_yaw_controller_t yaw_ctrl);

// start/stop auto tuner
void yctrl_autotune_start(mp_yaw_controller_t yaw_ctrl);
void yctrl_autotune_restore(mp_yaw_controller_t yaw_ctrl);

static inline bool yctrl_rate_control_enabled(mp_yaw_controller_t yaw_ctrl) { return yaw_ctrl->_rate_enable != 0; }

// return true if rate control or damping is enabled
static inline bool yctrl_enabled(mp_yaw_controller_t yaw_ctrl) { return yctrl_rate_control_enabled(yaw_ctrl) || (yaw_ctrl->_K_D > 0.0f); } 

// reduce the integrator, used when we have a low scale factor in a quadplane hover
// this reduces integrator by 95% over 2s
static inline void yctrl_decay_i(mp_yaw_controller_t yaw_ctrl) { yaw_ctrl->_pid_info.I *= 0.995f; }

static inline pid_info_const_t yctrl_get_pid_info(mp_yaw_controller_t yaw_ctrl) { return &yaw_ctrl->_pid_info; }
static inline float yctrl_get_kp(mp_yaw_controller_t yaw_ctrl) { return yaw_ctrl->rate_pid._kp; }
static inline float yctrl_get_ki(mp_yaw_controller_t yaw_ctrl) { return yaw_ctrl->rate_pid._ki; }
static inline float yctrl_get_kd(mp_yaw_controller_t yaw_ctrl) { return yaw_ctrl->rate_pid._kd; }
static inline float yctrl_get_kff(mp_yaw_controller_t yaw_ctrl) { return yaw_ctrl->rate_pid._kff; }
static inline float yctrl_get_tau(mp_yaw_controller_t yaw_ctrl) { return yaw_ctrl->gains.tau; }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



